#include <Trajectory.h>

ControlCommand::ControlCommand(double xv,double tv,int steps):_lv(xv),_av(tv),n_steps(steps){
}

ControlCommand::ControlCommand(){
	_lv = 0.0; _av = 0.0; n_steps = 1;
}

double ControlCommand::lv(double lv){
	_lv = lv != INF ? lv : _lv;
	return _lv; 
}
double ControlCommand::av(double av){
	_av = av != INF ? av : _av;
	return _av; 
}
int ControlCommand::number_steps(){
	return n_steps; 
}

Trajectory::Trajectory(ControlCommand &cc):ctrl(cc){
	ROS_INFO("Constructing trajectory");
	client = n.serviceClient<fnavfn::PlotTrajectory>("/trajectory_plot");
	clr  = n.serviceClient<fnavfn::Clear>("/clear_plots");
}

Trajectory::Trajectory(){
	client = n.serviceClient<fnavfn::PlotTrajectory>("/trajectory_plot");
	clr  = n.serviceClient<fnavfn::Clear>("/clear_plots");
}

double Trajectory::add_cost(double c){
	return ( cost += c );
}

double Trajectory::get_cost(){
	return cost;
}

void Trajectory::add_point(double hpos,double vpos,double theta){
	t.push_back(theta);
	x.push_back(hpos);
	y.push_back(vpos);	
}

TFFF Trajectory::get_point(int index){
	if( index >= (int)x.size() ) 
		return TFFF(INV,PFF(INV,INV));
	return TFFF(t[index],PFF(x[index],y[index]));
}

void Trajectory::clear(){
	x.clear();
	y.clear();
	t.clear();
	cost = 0;
}

int Trajectory::length(){
	return x.size();
}

void Trajectory::plot(bool special){
	srv.request.x = x;
	srv.request.y = y;
	srv.request.special = 0;
	if (!client.call(srv))
		; //ROS_ERROR("Plotting failed");

}

void Trajectory::clearViz(){
	if(!clr.call(sclr))
		; //ROS_ERROR("Clearing failed");
}
